﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;
using System.IO;

namespace NavmeshPathfinding
{
    public class Navmesh : DrawableGameComponent
    {
        public static bool reactionEnabled = false;

        Core core;
        SpriteBatch spriteBatch;
        SpriteFont spriteFont;

        BasicEffect basicEffect;
        VertexBuffer vertexBuffer;
        IndexBuffer indexBuffer;
        IndexBuffer lineIndexBuffer;

        VertexPositionColor[] vertexList;
        Dictionary<int, Dictionary<int, NavmeshEdge>> vertexEdgeMap;
        List<NavmeshPolygon> polygonList;
        List<NavmeshEdge> edgeList;
        int vertexNum;
        int indexNum;
        int lineIndexNum;
        int primitiveNum;

        DepthStencilState depthStencilState;

        Dictionary<Agent, NavmeshPolygon> agentPolygonMap;
        Dictionary<Agent, List<Vector3>> agentPath;
        Dictionary<Agent, List<NavmeshEdge>> agentEdgePath;
        Dictionary<Agent, List<NavmeshPolygon>> agentPolygonPath;
        Dictionary<Agent, List<Vector3>> agentActualPath;
        Dictionary<Agent, List<Shape>> agentPlannedPathLine;
        Dictionary<Agent, List<Shape>> agentActualPathLine;

        class PathInfo
        {
            public bool enabled = false;
            public List<NavmeshEdge> edgePath;
            public List<NavmeshPolygon> polygonPath;
        }

        Dictionary<NavmeshPolygon, Dictionary<NavmeshPolygon, PathInfo>> shortestPathCache;

        bool displayVertexInfo;

        public Navmesh(Core core) : base(core)
        {
            this.core = core;
            displayVertexInfo = true;
        }

        public override void Initialize()
        {
            base.Initialize();

            basicEffect = new BasicEffect(GraphicsDevice);
            basicEffect.VertexColorEnabled = true;

            depthStencilState = new DepthStencilState();
            depthStencilState.DepthBufferEnable = false;
        }

        public void loadNavmesh()
        {
            LoadContent();
        }

        protected override void LoadContent()
        {
            base.LoadContent();

            polygonList = new List<NavmeshPolygon>();
            edgeList = new List<NavmeshEdge>();
            agentPolygonMap = new Dictionary<Agent, NavmeshPolygon>();
            agentPath = new Dictionary<Agent, List<Vector3>>();
            agentEdgePath = new Dictionary<Agent, List<NavmeshEdge>>();
            agentActualPath = new Dictionary<Agent, List<Vector3>>();
            agentActualPathLine = new Dictionary<Agent, List<Shape>>();
            agentPlannedPathLine = new Dictionary<Agent, List<Shape>>();
            agentPolygonPath = new Dictionary<Agent, List<NavmeshPolygon>>();
            shortestPathCache = new Dictionary<NavmeshPolygon, Dictionary<NavmeshPolygon, PathInfo>>();

            List<int> indexList = new List<int>();
            List<int> lineIndexList = new List<int>();
            vertexEdgeMap = new Dictionary<int, Dictionary<int, NavmeshEdge>>();

            string line;
            string[] tokens;

            List<VertexPositionColor> vertexList = new List<VertexPositionColor>();
            using (StreamReader reader = new StreamReader(new FileStream("navmesh.nvm", FileMode.Open, FileAccess.Read)))
            {
                while(true)
                {
                    line = reader.ReadLine();
                    if (line == "") break;

                    tokens = line.Split(Config.Navmesh.NVM_FORMAT_DELIMITER);
                    vertexList.Add(new VertexPositionColor(new Vector3(float.Parse(tokens[0]), float.Parse(tokens[1]), float.Parse(tokens[2])), Config.Navmesh.NAVMESH_COLOR));
                }

                vertexNum = vertexList.Count;
                this.vertexList = new VertexPositionColor[vertexNum * 2];
                for (int i = 0; i < vertexList.Count; i++)
                    this.vertexList[i] = vertexList[i];

                for(int i=0; ;i++)
                {
                    if (reader.EndOfStream) break;

                    line = reader.ReadLine();
                    if (line == "") break;

                    tokens = line.Split(Config.Navmesh.NVM_FORMAT_DELIMITER);

                    int[] polygonIndexList = new int[tokens.Length];
                    for (int j = 0; j < tokens.Length; j++)
                        polygonIndexList[j] = Int32.Parse(tokens[j]);

                    NavmeshPolygon newPolygon = new NavmeshPolygon(this.vertexList, polygonIndexList);

                    //creates NavmeshEdge for each edge without duplication
                    for (int j = 0; j < polygonIndexList.Length; j++)
                    {
                        int vertex1, vertex2;

                        if (j == polygonIndexList.Length - 1)
                        {
                            vertex1 = polygonIndexList[j];
                            vertex2 = polygonIndexList[0];
                        }
                        else
                        {
                            vertex1 = polygonIndexList[j];
                            vertex2 = polygonIndexList[j + 1];
                        }

                        int min, max;
                        min = Math.Min(vertex1, vertex2);
                        max = Math.Max(vertex1, vertex2);

                        if (vertexEdgeMap.ContainsKey(min) && vertexEdgeMap[min].ContainsKey(max))
                        {
                            NavmeshEdge edge = vertexEdgeMap[min][max];

                            newPolygon.addEdge(edge);
                            edge.addPolygon(newPolygon);
                        }
                        else
                        {
                            Vector3 a = vertexList[min].Position;
                            Vector3 b = vertexList[max].Position;
                            NavmeshEdge newEdge = new NavmeshEdge(a,b);

                            if (!vertexEdgeMap.ContainsKey(min))
                                vertexEdgeMap[min] = new Dictionary<int, NavmeshEdge>();
                            vertexEdgeMap[min][max] = newEdge;

                            edgeList.Add(newEdge);
                            newPolygon.addEdge(newEdge);
                            newEdge.addPolygon(newPolygon);
                        }

                        //add line index list
                        lineIndexList.Add(vertex1 + vertexNum);
                        lineIndexList.Add(vertex2 + vertexNum);
                    }

                    polygonList.Add(newPolygon);
                }

                primitiveNum = polygonList.Count;
            }

            //classify each edge in polygon
            foreach (NavmeshPolygon polygon in polygonList)
            {
                shortestPathCache[polygon] = new Dictionary<NavmeshPolygon,PathInfo>();
                foreach (NavmeshPolygon polygon2 in polygonList)
                    shortestPathCache[polygon][polygon2] = new PathInfo();

                polygon.classifyEdges();
            }

            //fill neighbor edge list for each edge
            foreach (NavmeshEdge edge in edgeList)
                edge.findNeighborEdges();

            //add line vertex to vertexList
            for (int i = 0; i < vertexNum; i++)
                this.vertexList[i + vertexNum] = new VertexPositionColor(vertexList[i].Position, Config.Navmesh.NAVMESH_LINE_COLOR);

            vertexBuffer = new VertexBuffer(GraphicsDevice, typeof(VertexPositionColor), vertexNum * 2, BufferUsage.WriteOnly);
            vertexBuffer.SetData<VertexPositionColor>(this.vertexList);

            foreach (NavmeshPolygon polygon in polygonList)
                polygon.getIndexList(ref indexList);
            indexNum = indexList.Count;
            indexBuffer = new IndexBuffer(GraphicsDevice, IndexElementSize.ThirtyTwoBits, indexList.Count, BufferUsage.WriteOnly);
            indexBuffer.SetData<int>(indexList.ToArray());
            //Load to VertexBuffer

            lineIndexNum = lineIndexList.Count;
            lineIndexBuffer = new IndexBuffer(GraphicsDevice, IndexElementSize.ThirtyTwoBits, lineIndexNum, BufferUsage.WriteOnly);
            lineIndexBuffer.SetData<int>(lineIndexList.ToArray());

            //initialize Agent data structure
            foreach (Agent agent in core.agentList)
                addAgent(agent);

            spriteBatch = new SpriteBatch(GraphicsDevice);
            spriteFont = core.Content.Load<SpriteFont>("hudFont");
        }

        public void addAgent(Agent agent)
        {
            NavmeshPolygon polygon = polygonList[Core.random.Next(polygonList.Count)];
            agentPolygonMap[agent] = polygon;
            agent.setPosition(polygon.getRandomVertex());
            agentEdgePath[agent] = new List<NavmeshEdge>();
            agentPath[agent] = new List<Vector3>();
            agentActualPath[agent] = new List<Vector3>();
            agentActualPathLine[agent] = new List<Shape>();
            agentPlannedPathLine[agent] = new List<Shape>();
            agentPolygonPath[agent] = new List<NavmeshPolygon>();
        }

        public void removeAgent(Agent agent)
        {
            agentEdgePath.Remove(agent);
            agentPath.Remove(agent);
            agentActualPath.Remove(agent);
            agentActualPathLine.Remove(agent);
            agentPlannedPathLine.Remove(agent);
            agentPolygonPath.Remove(agent);
            agentPolygonMap.Remove(agent);
        }

        int agentActualPathLastTimeChecked;
        public override void Update(GameTime gameTime)
        {
            if (gameTime.TotalGameTime.TotalMilliseconds - agentActualPathLastTimeChecked > Config.Navmesh.ACTUAL_PATH_CHECK_PERIOD)
            {
                foreach (var pair in agentActualPath)
                {
                    Agent agent = pair.Key;
                    List<Vector3> path = pair.Value;

                    Vector3 position = agent.getPosition();
                    if (path.Count == 0 || !position.Equals(path.Last()))
                    {
                        path.Add(position);
                        if (path.Count > 1)
                        {
                            Vector3 lastPosition = path[path.Count - 2];

                            List<Shape> pathLine = agentActualPathLine[agent];
                            if (pathLine.Count > Config.Navmesh.ACTUAL_PATH_LINE_MAX_COUNT)
                                pathLine.RemoveAt(0);
                            pathLine.Add(new Line(GraphicsDevice, lastPosition + Config.Navmesh.PATH_OFFSET * 1.5f, position + Config.Navmesh.PATH_OFFSET, agentPolygonMap[agent].getNormal(), Config.Navmesh.ACTUAL_PATH_WIDTH, Config.Navmesh.ACTUAL_PATH_COLOR));
                        }
                    }
                }
                agentActualPathLastTimeChecked = (int) gameTime.TotalGameTime.TotalMilliseconds;
            }

            base.Update(gameTime);
        }

        public override void Draw(GameTime gameTime)
        {
            GraphicsDevice.DepthStencilState = DepthStencilState.Default;
            GraphicsDevice.SetVertexBuffer(vertexBuffer);
            GraphicsDevice.Indices = indexBuffer;
 
            basicEffect.World = Matrix.Identity;
            basicEffect.View = core.camera.getViewMatrix();
            basicEffect.Projection = core.camera.getProjectionMatrix();

            basicEffect.CurrentTechnique.Passes[0].Apply();

            GraphicsDevice.RasterizerState = RasterizerState.CullNone;
            GraphicsDevice.DrawIndexedPrimitives(PrimitiveType.TriangleList, 0, 0, vertexNum, 0, indexNum / 3);
            //GraphicsDevice.DrawIndexedPrimitives(PrimitiveType.LineStrip, 0, 0, vertexNum, 0, indexNum / 3);
            GraphicsDevice.RasterizerState = RasterizerState.CullCounterClockwise;

            GraphicsDevice.Indices = lineIndexBuffer;
            GraphicsDevice.DrawIndexedPrimitives(PrimitiveType.LineList, 0, vertexNum, vertexNum, 0, lineIndexNum / 2);

            //Draw path
            foreach (var pair in agentPlannedPathLine)
            {
                Agent agent = pair.Key;
                List<Shape> path = pair.Value;

                foreach (Shape shape in path)
                    shape.draw();
            }

            foreach (var pair in agentActualPathLine)
            {
                Agent agent = pair.Key;
                List<Shape> path = pair.Value;

                foreach (Shape shape in path)
                    shape.draw();
            }

            //printVertexInfo
            if (displayVertexInfo)
            {
                spriteBatch.Begin();
                for (int i = 0; i < vertexNum; i++)
                {
                    VertexPositionColor vpc = vertexList[i];
                    Vector3 position = vpc.Position;
                    Vector3 coordinates = GraphicsDevice.Viewport.Project(position, core.camera.getProjectionMatrix(), core.camera.getViewMatrix(), Matrix.Identity);
                    if (coordinates.X > 0 && coordinates.X <= 1024 && coordinates.Y > 0 && coordinates.Y <= 768 && coordinates.Z > 0 && coordinates.Z <= 1)
                    {
                        spriteBatch.DrawString(spriteFont, String.Format("{0}: ({1:0.#},{2:0.#},{3:0.#})", i, position.X, position.Y, position.Z), new Vector2(coordinates.X - 1, coordinates.Y - 1), Color.Black);
                        spriteBatch.DrawString(spriteFont, String.Format("{0}: ({1:0.#},{2:0.#},{3:0.#})", i, position.X, position.Y, position.Z), new Vector2(coordinates.X, coordinates.Y), Color.White);
                    }
                }
                spriteBatch.End();
            }
            base.Draw(gameTime);
        }

        public void toggleVertexInfoDisplay()
        {
            displayVertexInfo = !displayVertexInfo;
        }

        public Vector3 getInitialPosition(Agent agent)
        {
            return agentPolygonMap[agent].getRandomVertex();
        }

        Shape getPlannedPathLineShape(Vector3 start, Vector3 end, Vector3 normal)
        {
            return new Arrow(GraphicsDevice, start + Config.Navmesh.PATH_OFFSET, end + Config.Navmesh.PATH_OFFSET, normal, Config.Navmesh.PREDICTED_PATH_WIDTH, Config.Navmesh.PREDICTED_PATH_ARROW_WIDTH, Config.Navmesh.PREDICTED_PATH_ARROW_LENGTH, Config.Navmesh.PREDICTED_PATH_COLOR);
        }

        public void mouseRightClicked(Agent[] agentList, int x, int y)
        {
            Vector3 nearPoint = core.GraphicsDevice.Viewport.Unproject(new Vector3((float)x, (float)y, 0.0f), core.camera.getProjectionMatrix(), core.camera.getViewMatrix(), Matrix.Identity);
            Vector3 farPoint = core.GraphicsDevice.Viewport.Unproject(new Vector3((float)x, (float)y, 1.0f), core.camera.getProjectionMatrix(), core.camera.getViewMatrix(), Matrix.Identity);

            Vector3 direction = farPoint - nearPoint;
            direction.Normalize();
            Ray ray = new Ray(nearPoint,direction);

            List<NavmeshPolygon> candidateList = new List<NavmeshPolygon>();
            foreach(NavmeshPolygon polygon in polygonList)
            {
                if(polygon.isIntersecting(ray))
                    candidateList.Add(polygon);
            }

            NavmeshPolygon candidate = null;
            float min = float.MaxValue;
            foreach (NavmeshPolygon polygon in candidateList)
            {
                float dst = polygon.intersection(ray);
                if (dst < min)
                {
                    min = dst;
                    candidate = polygon;
                }
            }

            Vector3 targetPoint = nearPoint + min * direction;

            if (candidate != null)
            {
                foreach (Agent agent in agentList)
                    commandAgentMoveTo(agent, candidate, targetPoint);
            }
        }

        void commandAgentMoveTo(Agent agent, NavmeshPolygon dstPolygon, Vector3 dstVertex)
        {
            if (dstPolygon == agentPolygonMap[agent])
            {
                List<Vector3> path = agentPath[agent];
                path.Clear();
                path.Add(dstVertex);

                List<Shape> pathLine = agentPlannedPathLine[agent];
                pathLine.Clear();
                pathLine.Add(getPlannedPathLineShape(agent.getPosition(), dstVertex, agentPolygonMap[agent].getNormal()));
            }
            else AStarAlgorithm(agent, agent.getPosition(), agentPolygonMap[agent], dstVertex, dstPolygon);

            agent.move();
        }

        static float AStarHeuristic(object src, object dst)
        {
            Vector3 a, b;
            if (src.GetType() == typeof(Vector3))
                a = (Vector3) src;
            else
                a = ((NavmeshEdge) src).getMidpoint();

            if (dst.GetType() == typeof(Vector3))
                b = (Vector3)dst;
            else
                b = ((NavmeshEdge)dst).getMidpoint();

            return Vector3.Distance(a, b);
        }

        static void openVerticesAdd(SortedDictionary<float, List<Vector3>> data, float key, Vector3 value)
        {
            if (data.ContainsKey(key))
                data[key].Add(value);
            else
            {
                data[key] = new List<Vector3>();
                data[key].Add(value);
            }
        }

        static KeyValuePair<float, Vector3> openVerticesPop(SortedDictionary<float, List<Vector3>> data)
        {
            var pair = data.First();
            Vector3 v = pair.Value.First();
            pair.Value.Remove(v);

            if (pair.Value.Count == 0)
                data.Remove(pair.Key);

            return new KeyValuePair<float, Vector3>(pair.Key, v);
        }

        static bool openVerticesContainsValue(SortedDictionary<float, List<Vector3>> data, Vector3 value)
        {
            foreach (var pair in data)
                if(pair.Value.Contains(value))
                    return true;

            return false;
        }

        static void openVerticesUpdate(SortedDictionary<float, List<Vector3>> data, float key, Vector3 value)
        {
            float oldKey = 0.0f;
            foreach (var pair in data)
                if (pair.Value.Contains(value))
                    oldKey = pair.Key;

            data[oldKey].Remove(value);
            if (data[oldKey].Count == 0)
                data.Remove(oldKey);

            openVerticesAdd(data, key, value);
        }

        void AStarAlgorithm(Agent agent, Vector3 srcVertex, NavmeshPolygon srcPolygon, Vector3 dstVertex, NavmeshPolygon dstPolygon)
        {
            bool cacheExists = false;
            PathInfo cache = shortestPathCache[srcPolygon][dstPolygon];
            PathInfo cache2 = shortestPathCache[dstPolygon][srcPolygon];;
            bool indexInversed = false;

            /*if (!cache.enabled)
            {
                if (cache2.enabled)
                {
                    indexInversed = true;
                    cacheExists = true;
                }
            }
            else
            {
                cacheExists = true;
            }*/

            if (cacheExists)
            {
                if (indexInversed)
                {
                    agentEdgePath[agent] = new List<NavmeshEdge>();
                    agentPolygonPath[agent] = new List<NavmeshPolygon>();

                    for (int i = cache2.edgePath.Count - 1; i >= 0; i--)
                        agentEdgePath[agent].Add(cache2.edgePath[i]);
                    for (int i = cache2.polygonPath.Count - 1; i >= 0; i--)
                        agentPolygonPath[agent].Add(cache2.polygonPath[i]);

                    cache.polygonPath = agentPolygonPath[agent];
                    cache.edgePath = agentEdgePath[agent];
                }
                else
                {
                    agentEdgePath[agent] = cache.edgePath;
                    agentPolygonPath[agent] = cache.polygonPath;
                }

                List<Vector3> path = new List<Vector3>();
                List<NavmeshEdge> edgePath = agentEdgePath[agent];

                path.Add(srcVertex);
                foreach (NavmeshEdge edge in edgePath)
                    path.Add(edge.getMidpoint());
                path.Add(dstVertex);
                agentPath[agent] = path;

                List<Shape> pathLine = agentPlannedPathLine[agent];
                pathLine.Clear();

                NavmeshPolygon curPolygon = srcPolygon;
                for (int i = 0; ; i++)
                {
                    pathLine.Add(getPlannedPathLineShape(path[i], path[i + 1], curPolygon.getNormal()));
                    if (i == path.Count - 2) break;
                    curPolygon = edgePath[i].getPolygonOtherThan(curPolygon);
                }
            }
            else
            {
                List<Vector3> path = new List<Vector3>();
                List<NavmeshEdge> edgePath = new List<NavmeshEdge>();
                List<NavmeshPolygon> polygonPath = new List<NavmeshPolygon>();

                List<Vector3> closedVertices = new List<Vector3>();
                SortedDictionary<float, List<Vector3>> openVertices = new SortedDictionary<float, List<Vector3>>();

                Dictionary<Vector3, Vector3> cameFrom = new Dictionary<Vector3, Vector3>();
                Dictionary<Vector3, float> costMap = new Dictionary<Vector3, float>();
                Dictionary<Vector3, NavmeshEdge> midpointEdgeMap = new Dictionary<Vector3, NavmeshEdge>();
                Vector3 curVertex = Vector3.Zero;
                NavmeshEdge curEdge = null;
                NavmeshEdge[] neighboringEdgeList;

                midpointEdgeMap[srcVertex] = null;
                midpointEdgeMap[dstVertex] = null;

                costMap[srcVertex] = 0;
                openVerticesAdd(openVertices, 0, srcVertex);
                while (openVertices.Count > 0)
                {
                    curVertex = openVerticesPop(openVertices).Value;
                    closedVertices.Add(curVertex);
                    curEdge = midpointEdgeMap[curVertex];

                    if (curVertex == dstVertex)
                        break;

                    bool considerDstVertex = false;
                    if (curEdge == null)
                    {
                        if (curVertex == srcVertex)
                            neighboringEdgeList = srcPolygon.getNeighboringEdgeList();
                        else if (curVertex == dstVertex)
                            neighboringEdgeList = dstPolygon.getNeighboringEdgeList();
                        else
                            throw new Exception("AStarAlgorithm: unhandled case");
                    }
                    else
                    {
                        if (dstPolygon.getNeighboringEdgeList().Contains(curEdge))
                            considerDstVertex = true;

                        neighboringEdgeList = curEdge.getNeighboringEdgeList();
                    }

                    foreach (NavmeshEdge edge in neighboringEdgeList)
                    {
                        Vector3 neighborVertex = edge.getMidpoint();

                        if (closedVertices.Contains(neighborVertex))
                            continue;

                        float distance;
                        if (curVertex == srcVertex)
                            distance = Vector3.Distance(curVertex, neighborVertex);
                        else if (curVertex == dstVertex)
                            distance = Vector3.Distance(curVertex, neighborVertex);
                        else
                            distance = curEdge.getDistanceTo(edge);

                        float cost = costMap[curVertex] + distance;
                        if (!openVerticesContainsValue(openVertices, neighborVertex) || cost <= costMap[neighborVertex])
                        {
                            costMap[neighborVertex] = cost;
                            cameFrom[neighborVertex] = curVertex;
                            float heuristicCost = cost + AStarHeuristic(edge, dstVertex);

                            if (!openVerticesContainsValue(openVertices, neighborVertex))
                            {
                                openVerticesAdd(openVertices, heuristicCost, neighborVertex);
                                midpointEdgeMap[neighborVertex] = edge;
                            }
                            else
                            {
                                openVerticesUpdate(openVertices, heuristicCost, neighborVertex);
                            }
                        }
                    }

                    if (considerDstVertex)
                    {
                        if (closedVertices.Contains(dstVertex))
                            continue;

                        float cost = costMap[curVertex] + Vector3.Distance(curVertex, dstVertex);
                        if (!openVerticesContainsValue(openVertices, dstVertex) || cost <= costMap[dstVertex])
                        {
                            costMap[dstVertex] = cost;
                            cameFrom[dstVertex] = curVertex;
                            float heuristicCost = cost + AStarHeuristic(dstVertex, dstVertex);

                            if (!openVerticesContainsValue(openVertices, dstVertex))
                            {
                                openVerticesAdd(openVertices, heuristicCost, dstVertex);
                            }
                        }
                    }
                }

                //if (curVertex != dstVertex)
                //throw new Exception("AStarAlgorithm: failure");

                path.Insert(0, curVertex);
                while (cameFrom.ContainsKey(curVertex))
                {
                    curVertex = cameFrom[curVertex];
                    path.Insert(0, curVertex);

                    if (midpointEdgeMap.ContainsKey(curVertex) && midpointEdgeMap[curVertex] != null)
                        edgePath.Insert(0, midpointEdgeMap[curVertex]);
                }

                List<Shape> pathLine = agentPlannedPathLine[agent];
                pathLine.Clear();

                NavmeshPolygon curPolygon = srcPolygon;
                polygonPath.Add(curPolygon);
                for (int i = 0; ; i++)
                {
                    pathLine.Add(getPlannedPathLineShape(path[i], path[i + 1], curPolygon.getNormal()));
                    if (i == path.Count - 2) break;
                    curPolygon = edgePath[i].getPolygonOtherThan(curPolygon);
                    polygonPath.Add(curPolygon);
                }
                polygonPath.Add(dstPolygon);

                agentPath[agent] = path;
                agentEdgePath[agent] = edgePath;
                agentPolygonPath[agent] = polygonPath;

                /*cache.enabled = true;
                cache.edgePath = edgePath;
                cache.polygonPath = polygonPath;*/
            }
        }

        public bool doesPathExistFor(Agent agent)
        {
            return !agentPath.ContainsKey(agent) || agentPath[agent].Count!=0;
        }

        static Vector3 flatten(Vector3 v)
        {
            return new Vector3(v.X, 0, v.Z);
        }

        bool isDestinationInSightFor(Agent agent, Vector3 target)
        {
            Vector3 pos = flatten(agent.getPosition());
            Vector3 flatTarget = flatten(target);
            List<NavmeshEdge> edgePath = agentEdgePath[agent];
            List<NavmeshPolygon> polygonPath = agentPolygonPath[agent];
            NavmeshPolygon curPolygon = agentPolygonMap[agent];
            bool isFastForwardComplete = false;
            NavmeshPolygon dstPolygon = polygonPath.Last();

            List<NavmeshPolygon> finishedList = new List<NavmeshPolygon>();

            foreach (NavmeshEdge edge in edgePath)
            {
                NavmeshPolygon[] polygonList = edge.getPolygonList();

                if (!isFastForwardComplete)
                {
                    if (polygonList.Contains(curPolygon))
                        isFastForwardComplete = true;
                    else continue;
                }

                foreach (NavmeshPolygon polygon in polygonList)
                {
                    if (finishedList.Contains(polygon))
                        continue;

                    foreach (NavmeshEdge testEdge in polygon.getEdgeList())
                    {
                        bool ans = testLineIntersection(flatten(testEdge.getVertex1()), flatten(testEdge.getVertex2()), pos, flatTarget);
                        if(ans)
                        {
                            if (testEdge.getPolygonCount() == 1)
                                return false;
                            else
                            {
                                NavmeshPolygon otherPolygon = testEdge.getPolygonOtherThan(polygon);

                                //if (otherPolygon == dstPolygon)
                                //    return true;
                                if (!polygonPath.Contains(otherPolygon))
                                    return false;
                            }
                        }
                    }

                    finishedList.Add(polygon);
                }
            }

            return true;
        }

        public Vector3 getReaction(Agent agent, Vector3 direction)
        {
            if (reactionEnabled)
            {
                float width = Config.Navmesh.REACTION_BOUNDARY_WIDTH;
                float length = Config.Navmesh.REACTION_BOUNDARY_LENGTH;
                Vector3 pos = agent.getPosition();
                Vector3 horizontal = Vector3.Cross(direction, Vector3.Up);
                horizontal.Normalize();
                Vector3 leftBottom = flatten(pos - horizontal * width / 2);
                Vector3 rightBottom = flatten(pos + horizontal * width / 2);
                Vector3 centerTop = flatten(pos + direction * length);
                Vector3 leftTop = centerTop - horizontal * width / 2;
                Vector3 rightTop = centerTop + horizontal * width / 2;

                bool rightObjectDetected = false;
                bool leftObjectDetected = false;

                List<NavmeshPolygon> polygonCheckList = new List<NavmeshPolygon>();

                NavmeshPolygon agentPolygon = agentPolygonMap[agent];
                polygonCheckList.Add(agentPolygon);
                foreach (NavmeshEdge edge in agentPolygon.getNeighboringEdgeList())
                    polygonCheckList.Add(edge.getPolygonOtherThan(agentPolygon));

                foreach (NavmeshPolygon polygon in polygonCheckList)
                {
                    if (leftObjectDetected && rightObjectDetected)
                        break;

                    foreach (NavmeshEdge edge in polygon.getNonNeighboringEdgeList())
                    {
                        if (leftObjectDetected && rightObjectDetected)
                            break;

                        Vector3 v1 = flatten(edge.getVertex1());
                        Vector3 v2 = flatten(edge.getVertex2());

                        if (!rightObjectDetected && (testLineIntersection(v1, v2, rightBottom, rightTop) || testLineIntersection(v1, v2, rightTop, centerTop)))
                            rightObjectDetected = true;
                        if (!leftObjectDetected && (testLineIntersection(v1, v2, leftBottom, leftTop) || testLineIntersection(v1, v2, leftTop, leftTop)))
                            leftObjectDetected = true;
                    }
                }

                if ((leftObjectDetected && rightObjectDetected) || (!leftObjectDetected && !rightObjectDetected))
                    return Vector3.Zero;
                else if (leftObjectDetected)
                    return horizontal;
                else
                    return -horizontal;
            }
            else
                return Vector3.Zero;
        }

        private Vector3 checkBoundedPosition(Agent agent, Vector3 src, Vector3 dst, NavmeshEdge edge)
        {
            NavmeshPolygon polygon = agentPolygonMap[agent];
            Vector3 newPosition;
            Vector3 v = dst - src;
            Vector3 e = edge.getVertex2() - edge.getVertex1();
            e.Normalize();

            float size_b = Vector3.Dot(v,e);
            Vector3 b = e * size_b;

            Vector3 boundingVertex = (size_b > 0) ? edge.getVertex2() : edge.getVertex1();

            if (b.Length() > Vector3.Distance(boundingVertex, src))
            {
                NavmeshEdge boundingEdge = null;
                foreach (NavmeshEdge _edge in polygon.getEdgeList())
                {
                    if (_edge == edge)
                        continue;

                    if (_edge.getVertex1() == boundingVertex || _edge.getVertex2() == boundingVertex)
                    {
                        boundingEdge = _edge;
                        break;
                    }
                }

                Vector3 intersect;
                if (boundingEdge == null)
                    throw new Exception("checkBoundedPosition: incosistency");
                else if (testLineIntersection(boundingEdge.getVertex1(), boundingEdge.getVertex2(), src, src + b))
                {
                    intersect = pointOfIntersection(boundingEdge.getVertex1(), boundingEdge.getVertex2(), src, src + b);
                    if (polygon.getNeighboringEdgeList().Contains(boundingEdge))
                    {
                        agentPolygonMap[agent] = boundingEdge.getPolygonOtherThan(polygon);
                        newPosition = checkPosition(agent, intersect, src + b);
                    }
                    else
                    {
                        newPosition = intersect;
                    }
                }
                else newPosition = checkPosition(agent, src, src + b);
            }
            else newPosition = src + b;

            return newPosition;
        }

        private Vector3 checkPosition(Agent agent, Vector3 position, Vector3 nextPosition)
        {
            if (!agentPolygonMap.ContainsKey(agent))
                return position;
            NavmeshPolygon polygon = agentPolygonMap[agent];
            Vector3 newPosition = nextPosition;

            //checks whether newPosition is on the plane of agent's Polygon. If not calibrate.
            if (!polygon.isVertexOnPlane(newPosition))
                newPosition = polygon.getCalibratedPositionOnPlane(position, newPosition);

            //checks whether newPosition-position segment crosses any of the agent's current polygon's edges
            NavmeshEdge crossingEdge = null;

            int[] polygonIndexList = polygon.getIndexList();
            for(int i=0; i<polygonIndexList.Length; i++)
            {
                int firstIndex = i;
                int secondIndex = (i==polygonIndexList.Length-1)?0:i+1;

                Vector3 crossProduct = Vector3.Cross(newPosition - position, vertexList[polygonIndexList[secondIndex]].Position - vertexList[polygonIndexList[firstIndex]].Position);
                if (crossProduct.Y < 0)
                {
                    int minVertex = Math.Min(polygonIndexList[secondIndex], polygonIndexList[firstIndex]);
                    int maxVertex = Math.Max(polygonIndexList[secondIndex], polygonIndexList[firstIndex]);
                    NavmeshEdge edge = vertexEdgeMap[minVertex][maxVertex];
                    if (testLineIntersection(flatten(position), flatten(newPosition), flatten(edge.getVertex1()), flatten(edge.getVertex2())))
                    {
                        crossingEdge = edge;
                        break;
                    }
                }
            }

            Vector3 newPosition2;
            //some edge is crossed. 
            if (crossingEdge != null)
            {
                if (polygon.getNeighboringEdgeList().Contains(crossingEdge))
                {
                    Vector3 intersect = pointOfIntersection(crossingEdge.getVertex1(), crossingEdge.getVertex2(), position, newPosition);
                    NavmeshPolygon newPolygon = crossingEdge.getPolygonOtherThan(polygon);
                    agentPolygonMap[agent] = newPolygon;
                    newPosition2 = checkPosition(agent, intersect, newPosition);
                }
                else
                {
                    Vector3 intersect = pointOfIntersection(crossingEdge.getVertex1(), crossingEdge.getVertex2(), position, newPosition);
                    newPosition2 = checkBoundedPosition(agent, intersect, newPosition, crossingEdge);
                }
            }
            else newPosition2 = newPosition;

            if (float.IsNaN(newPosition2.X) || float.IsNaN(newPosition2.Y) || float.IsNaN(newPosition2.Z))
                throw new Exception("newPosition is NaN");

            return newPosition2;
        }

        public Vector3 checkPosition(Agent agent, Vector3 nextPosition)
        {
            return checkPosition(agent, agent.getPosition(), nextPosition);
        }

        public Vector3 getCurrentDestination(Agent agent)
        {
            if (!agentPath.ContainsKey(agent))
                return Vector3.Zero;

            List<Vector3> path = agentPath[agent];

            if (path.Count == 0)
                throw new Exception("getCurrentDestination: there must be at least one path");
            if (path.Count == 1)
                return path[0];

            if (isDestinationInSightFor(agent, path[1]))
            {
                Vector3 dst = path[1];
                path.RemoveAt(0);
                return dst;
            }
            else 
                return path[0];
        }

        public void requestRandomDestination(Agent agent)
        {
            NavmeshPolygon polygon = polygonList[Core.random.Next(polygonList.Count)];
            commandAgentMoveTo(agent, polygon, polygon.getRandomVertex());
        }

        public void currentDestinationReached(Agent agent)
        {
            agentPath[agent].RemoveAt(0);
        }

        private class NavmeshPolygon //should be in a plane
        {
            List<NavmeshEdge> edgeList;
            List<NavmeshEdge> neighboringEdgeList;
            List<NavmeshEdge> nonNeighboringEdgeList;
            VertexPositionColor[] vertexList;
            int[] indexList;
            Vector3 normal;
            Vector3 center;
            float d;

            public NavmeshPolygon(VertexPositionColor[] vertexList, int[] indexList)
            {
                this.vertexList = vertexList;
                this.indexList = indexList;
                edgeList = new List<NavmeshEdge>();
                neighboringEdgeList = new List<NavmeshEdge>();
                nonNeighboringEdgeList = new List<NavmeshEdge>();
                normal = Vector3.Zero;

                findNormal();
                d = Vector3.Dot(normal, vertexList[indexList[0]].Position);

                Vector3 sum = Vector3.Zero;
                for (int i = 0; i < indexList.Length; i++)
                    sum += vertexList[indexList[i]].Position;
                sum /= indexList.Length;
                center = sum;
            }

            public Vector3 getRandomVertex()
            {
                Random random = Core.random;
                Vector3 v1 = vertexList[indexList[random.Next(indexList.Length)]].Position;
                Vector3 v2 = center;

                return (v2 - v1) * ((float)random.NextDouble()) + v1;
            }

            public Vector3 getNormal()
            {
                return normal;
            }

            void findNormal()
            {
                for(int i=1; i<indexList.Length-1&&Mathf.isEqual(normal,Vector3.Zero) ; i++)
                {
                    Vector3 a = vertexList[indexList[i-1]].Position - vertexList[indexList[i]].Position;
                    Vector3 b = vertexList[indexList[i+1]].Position - vertexList[indexList[i]].Position;

                    normal = Vector3.Cross(a, b);
                }

                normal.Normalize();
            }

            public void getIndexList(ref List<int> indexList)
            {
                int[] ind = this.indexList;

                for (int i = 1; i < ind.Length - 1; i++)
                {
                    indexList.Add(ind[0]);
                    indexList.Add(ind[i]);
                    indexList.Add(ind[i + 1]);
                }
            }

            public void addEdge(NavmeshEdge edge)
            {
                edgeList.Add(edge);
            }

            public void classifyEdges()
            {
                foreach (NavmeshEdge edge in edgeList)
                {
                    if (edge.getPolygonCount() == 2)
                        neighboringEdgeList.Add(edge);
                    else if (edge.getPolygonCount() == 1)
                        nonNeighboringEdgeList.Add(edge);
                    else throw new Exception(String.Format("edge has {0} neighboring polygons", edge.getPolygonCount()));
                }
            }

            public NavmeshEdge[] getEdgeList()
            {
                return edgeList.ToArray();
            }

            public NavmeshEdge[] getNeighboringEdgeList()
            {
                return neighboringEdgeList.ToArray();
            }

            public NavmeshEdge[] getNonNeighboringEdgeList()
            {
                return nonNeighboringEdgeList.ToArray();
            }

            public bool isIntersecting(Ray ray)
            {
                float t = intersection(ray);
                for (int i = 1; i < indexList.Length - 1; i++)
                {
                    Vector3 v0 = vertexList[indexList[0]].Position;
                    Vector3 v1 = vertexList[indexList[i]].Position;
                    Vector3 v2 = vertexList[indexList[i + 1]].Position;

                    if (isIntersectingTriangle(ray, normal, v0, v1, v2, t))
                        return true;
                }

                return false;
            }

            public int[] getIndexList()
            {
                return indexList;
            }

            public bool isVertexOnPlane(Vector3 x)
            {
                return Vector3.Dot(x, normal) == d;
            }


            //finds the normal of the plane that contains the normal of the polygon and dst-src
            //cross products the two normals
            //finds a point on the intersection that has the same distance from the src Vector.
            public Vector3 getCalibratedPositionOnPlane(Vector3 src, Vector3 dst)
            {
                Vector3 v = dst - src;
                if (Mathf.abs(v.Length()) < Mathf.EPSILON) return dst;

                Vector3 n = Vector3.Cross(v, normal);
                n.Normalize();
                Vector3 u = Vector3.Cross(normal, n);
                u.Normalize();
                
                return src + v.Length() * u;
            }

            static float comp(Vector3 v, int i) //getComponent
            {
                if (i == 0)
                    return v.X;
                else if (i == 1)
                    return v.Y;
                else
                    return v.Z;
            }

            private bool isIntersectingTriangle(Ray ray, Vector3 N, Vector3 V0, Vector3 V1, Vector3 V2, float t)
            {
                if (Mathf.isEqual(Vector3.Dot(N, ray.Direction),0))
                    return false;

                if (t <= 0)
                    return false;

                int i1, i2;
                float maxN = Mathf.max(Mathf.max(Mathf.abs(N.X),Mathf.abs(N.Y)),Mathf.abs(N.Z));
                float a, b, u0, u1, u2, v0, v1, v2;
                Vector3 P;

                if(Mathf.isEqual(Mathf.abs(N.X),maxN))
                {
                    i1 = 1;
                    i2 = 2;
                }
                else if (Mathf.isEqual(Mathf.abs(N.Y),maxN))
                {
                    i1 = 0;
                    i2 = 2;
                }
                else
                {
                    i1 = 1;
                    i2 = 0;
                }

                P = ray.Position + ray.Direction * t;
                u0 = comp(P, i1) - comp(V0, i1);
                v0 = comp(P, i2) - comp(V0, i2);
                u1 = comp(V1, i1) - comp(V0, i1);
                u2 = comp(V2, i1) - comp(V0, i1);
                v1 = comp(V1, i2) - comp(V0, i2);
                v2 = comp(V2, i2) - comp(V0, i2);

                a = 0;
                if (Mathf.isEqual(u1,0))
                {
                    b = u0 / u2;
                    if (0 <= b && b <= 1)
                        a = (v0 - b * v2) / v1;
                }
                else
                {
                    b = (v0 * u1 - u0 * v1) / (v2 * u1 - u2 * v1);
                    if (0 <= b && b <= 1)
                        a = (u0 - b * u2) / u1;
                }

                return (b <= 1 && a >= 0 && b >= 0 && (a + b) <= 1);
            }

            public float intersection(Ray ray)
            {
                float vn = Vector3.Dot(ray.Direction, normal);

                if (Mathf.isEqual(vn,0))
                    return float.PositiveInfinity;
                else
                    return (d - Vector3.Dot(ray.Position, normal)) / vn;
            }
        }

        private class NavmeshEdge
        {
            Vector3 a,b;
            Vector3 midpoint;
            VertexPositionColor midpointVPC;
            List<NavmeshPolygon> neighborPolygonList;
            List<NavmeshEdge> neighborEdgeList;
            Dictionary<NavmeshEdge, float> edgeDistanceMap;

            public NavmeshEdge(Vector3 a, Vector3 b)
            {
                this.a = a;
                this.b = b;
                midpoint = (a + b) / 2;
                midpointVPC = new VertexPositionColor(midpoint, Config.Navmesh.PREDICTED_PATH_COLOR);
                neighborEdgeList = new List<NavmeshEdge>();
                neighborPolygonList = new List<NavmeshPolygon>();
                edgeDistanceMap = new Dictionary<NavmeshEdge, float>();
            }

            public Vector3 getVertex1()
            {
                return a;
            }

            public Vector3 getVertex2()
            {
                return b;
            }

            public Vector3 getMidpoint()
            {
                return midpoint;
            }

            public VertexPositionColor getMidpointVPC()
            {
                return midpointVPC;
            }

            public void addPolygon(NavmeshPolygon polygon)
            {
                neighborPolygonList.Add(polygon);
            }

            public int getPolygonCount()
            {
                return neighborPolygonList.Count;
            }

            public NavmeshPolygon[] getPolygonList()
            {
                return neighborPolygonList.ToArray();
            }

            public NavmeshEdge[] getNeighboringEdgeList()
            {
                return neighborEdgeList.ToArray();
            }

            public NavmeshPolygon getPolygonOtherThan(NavmeshPolygon polygon)
            {
                foreach (NavmeshPolygon p in neighborPolygonList)
                {
                    if (p != polygon)
                        return p;
                }

                throw new Exception("getPolygonOtherThan: no polygon found");
            }

            public void findNeighborEdges()
            {
                foreach (NavmeshPolygon polygon in neighborPolygonList)
                    foreach (NavmeshEdge edge in polygon.getNeighboringEdgeList())
                    {
                        if (edge == this) continue;
                        neighborEdgeList.Add(edge);
                        edgeDistanceMap[edge] = Vector3.Distance(getMidpoint(), edge.getMidpoint());
                    }
            }

            public float getDistanceTo(NavmeshEdge edge)
            {
                return edgeDistanceMap[edge];
            }
        }

        static bool testLineIntersection(Vector3 a1, Vector3 a2, Vector3 b1, Vector3 b2)
        {
            Vector3 n = Vector3.Cross(a2 - a1, b2 - b1);
            if (Mathf.isEqual(Mathf.abs(n.Length()), 0))
                return false;

            n.Normalize();
            float d = Vector3.Dot(a1, n);

            if (!Mathf.isEqual(Vector3.Dot(b1, n),d))
                return false;


            Vector3 v = a2 - a1;
            Vector3 u = b2 - b1;
            float r = (v.X * u.Z - u.X * v.Z);
            float s = ((a1.Z - b1.Z) * v.X + (b1.X - a1.X) * v.Z) / r;
            float t = ((a1.Z - b1.Z) * u.X + (b1.X - a1.X) * u.Z) / r;

            return (0 <= s && s <= 1 && 0 <= t && t <= 1);
        }

        static Vector3 pointOfIntersection(Vector3 a1, Vector3 a2, Vector3 b1, Vector3 b2)
        {
            Vector3 v = a2 - a1;
            Vector3 u = b2 - b1;
            float s = ((a1.Z - b1.Z) * v.X + (b1.X - a1.X) * v.Z) / (v.X * u.Z - u.X * v.Z);

            return b1 + u * s;
        }
    }
}
